#include "RTthreads.h"

#include <string.h>
#include <pthread.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include "RTthreads.h"
#include "tools_share.h"
#include "test_share_mem.h"

struct cylic_thread_params ctp_fast_task;
struct once_thread_params otp_ECM_INIT;
struct waitsem_thread_params wtsem_slow_follow_task1;
struct waitsem_thread_params wtsem_slow_follow_task2;

#define PROGRM_MAIN_PATH    "EXO_MAIN_PATH" //系统的变量名称

int counterb=0;
void CylicThread_DATA_exchange(struct cylic_thread_params *f_p)
{
    locksem(sbf.semid,USR_SEMID_RT_STATUS);
    sbf_pdata->rt_data.error_STATUS=counterb;
    printf("%d,%d,",sbf_pdata->rt_data.FSM_State,sbf_pdata->rt_data.error_STATUS);
    unlocksem(sbf.semid,USR_SEMID_RT_STATUS);


    locksem(sbf.semid,USR_SEMID_RT_SERVO_CTRL);
    printf("%f,%f,%f\n",
    sbf_pdata->rt_data.model_Q[1],
    sbf_pdata->rt_data.model_Q[2],
    sbf_pdata->rt_data.model_Q[3]);

    unlocksem(sbf.semid,USR_SEMID_RT_SERVO_CTRL);

    counterb+=2;
}



int main() 
{
    int i;

//获取程序目录\环境变量：
	char * s = getenv(PROGRM_MAIN_PATH);
	memcpy(s_PROGRM_MAIN_PATH,s,strlen(s));

//建立共享内存：
    test_sharemem_client_build(&sbf,135);

/*建立基频倍频线程START*/
	InitCylicThread(&ctp_fast_task);
	ctp_fast_task.thread_id = 4;
	ctp_fast_task.period_ns = 100*1000000;//10ms
	ctp_fast_task.cyclic_task = CylicThread_DATA_exchange;
	ctp_fast_task.ctrl = 0; // OnceThread_ECM _INIT启动完成后，再执行本线程的内容
	ctp_fast_task.timeout_warn=0;
	ctp_fast_task.CPU_idx = 1; //
	ctp_fast_task.priority=83;
    build_RTCylic(&ctp_fast_task);//
	append_Cylic(&ctp_fast_task);

    sleep(1);
    *(ctp_fast_task.p_ctrl) = 1;//打开ctp_fast_task线程

    int cnt=0;
    while (cnt<60000)
    {
        sleep(1);
        cnt++;
    }
    clearAllCylicThread();
    sleep(1);


    return 1;

}
